using System;
using System.IO;
using System.Collections;
using System.Collections.Generic;
using System.Linq;
using UnityEngine;
using DeepLearning;
using System.Threading.Tasks;
using Assets.Scripts.LDS;


public class RootInfo
{
    public Matrix4x4[] Transformations;
    public Vector3[] Velocities;
}

public class BoneInfo
{
    public Matrix4x4 TransformMat = new Matrix4x4();
    public Vector3 Velocity = new Vector3();
    public Vector3 Acceleration = new Vector3();
    public Vector3 Force = new Vector3();
    public int Index;
    public int Parent;
    public int[] Childs;
    public float Length;
}

public class FeedNSMInfo
{
    public Controller Controller;
    public TimeSeries TimeSeries;
    public TimeSeries.Style StyleSeries;
    public TimeSeries.Goal GoalSeries;
    public TimeSeries.Contact ContactSeries;
    public TimeSeries.Phase PhaseSeries;
    public CylinderMap Environment;
    public CuboidMap Geometry;

    public Vector3[] PosePrediction;
    public Matrix4x4[] RootPrediction;
    public Matrix4x4[] GoalPrediction;

    public TimeSeries.Root RootSeries;

    public List<BoneInfo> boneInfo = new List<BoneInfo>();

    public RootInfo rootInfo;

    //*******************************************************************

    public List<Vector3> bones = new List<Vector3>();
    public List<Vector3> trajs = new List<Vector3>();
    public List<float[]> styles = new List<float[]>();

    public List<Vector3> goals = new List<Vector3>();
    public List<float[]> styGoals = new List<float[]>();
    public float[] env;

    public List<Vector3> geoms = new List<Vector3>();
    public List<float> geomsOcc = new List<float>();

    public float[] gats;
}

public class StatInfo
{
    // public Actor actor;  // Bone[] Bones
    public List<Matrix4x4> routes;

    public List<Matrix4x4> navRoutes;

    // The action to be performed, the corresponding interaction and the information of the connected object
    // Stored is the index in info.models (index is the only value that can determine the model, name and so on may have the same name)
    // To obtain the transform of the model, you can use info.models[index].model.transform
    public ActionInfo action;
    public List<int> targets;
    public List<int> styleTargets;

    public Dictionary<int, int> targetObj;
    public Dictionary<int, int> styleTargetObj;

    public Dictionary<int, Matrix4x4> objTrans;

    //public Dictionary<string, Matrix4x4> allObjTrans;  //The position of all objects in the scene

    public Matrix4x4 model; // The location of the character model
    public Matrix4x4 nsm; // The location of Anbius

    public Dictionary<string, Matrix4x4> bones; // Bone[] Bones Stored bone point information

    public Dictionary<string, Matrix4x4> smodel; // the transform of Anubis and all its sub-objects to distinguish bones
    public FeedNSMInfo snsmInfo; // At a certain moment, Anubis input to the network status information float[]

    public Dictionary<GameObject, (HashSet<BoxCollider>, int)> collisionDepth;

    public List<Matrix4x4> nsmOptimRoutes;

    public Dictionary<string, Matrix4x4> lastOptimFrame;

    public StatInfo()
    {
        bones = null;
        routes = null;
        //interObjIDs = null;
        //contObjIDs = null;
        objTrans = null;
        smodel = null;
        collisionDepth = null;
        nsmOptimRoutes = null;
        lastOptimFrame = null;
    }


    public StatInfo(StatInfo si)
    {
        // actor = si.actor;  // Bone[] Bones 存储了骨骼点的信息
        routes = si.routes;

        action = si.action;
        targets = si.targets;
        styleTargets = si.styleTargets;
        targetObj = si.targetObj;
        styleTargetObj = si.styleTargetObj;
        objTrans = si.objTrans;

        model = si.model;
        nsm = si.nsm;
        bones = si.bones;

        smodel = si.smodel;
        navRoutes = si.navRoutes;
        collisionDepth = si.collisionDepth;
        nsmOptimRoutes = si.nsmOptimRoutes;
        lastOptimFrame = si.lastOptimFrame;
    }
}


public class ProcessText : MonoBehaviour
{
    static public bool IsChanged = false;

    static int ModelCount = 0;

    public bool OptimizeOn = true;

    public Animator motionAvatar;
    public bool drawTrajectory = true;

    // public OptimProgram optimSA;
    public Info info = new Info();

    // Update action after optimization
    [HideInInspector] public bool IsChangedOptim = false;


    //********************************
    // 缓冲队列信息 

    [HideInInspector] public StatInfo bufferOld;

    // Begin State
    private StatInfo bufferCur;
    // End State


    private ActionInfo bufferAction;

    private bool actNavCompleted = false;
    private bool actOptimCompleted = false;
    private bool actNSMCompleted = false;
    private bool actResetCompleted = false;

    private bool stayingActionBreak = false;

    private GameObject tarInterModel;
    private GameObject tarStyleModel;


    //********************************
    // optim params
    private float bestErrorFound;
    private System.Random seed = new System.Random();

    private StatInfo optimOld;

    [HideInInspector] public StatInfo optimCur;
    // Variable State during iteration

    // process flag
    private bool isOptim = false; //The whole optimization
    private bool isSAOptim = false; //Iterative optimization
    private bool isResetOptim = false;
    private bool perOptimCompleted = false;


    [HideInInspector] public bool
        isNSMOptim =
            false; //In iterative optimization, the optimized action is handed over to the NSM execution process


    //********************************
    [HideInInspector] public ActionInfo action;

    [HideInInspector] public int actionNum = 0;

    [HideInInspector] public int IdxAct = 0;


    public static void OnChanged()
    {
        // set inchanged true when LDS generate a result
        IsChanged = true;
    }

    public List<Matrix4x4> getFutureRoutine(Matrix4x4 root, float gap, ActionInfo taction)
    {
        int step = 10; // 方向点距离
        int tarIdx = 1; // 目标位置的下标
        List<Matrix4x4> res = new List<Matrix4x4>();
        List<Matrix4x4> pre = taction.routesOptim;

        //Debug.Log(gap);

        if (gap < 0.1f)
        {
            if (bufferAction.actASC == (byte) ActionType.Idle)
            {
                tarIdx = 0;
            }
            else if (bufferAction.actASC == (byte) ActionType.Run)
            {
                tarIdx = 4;
                gap = 0.1f;
            }
            else if (bufferAction.actASC == (byte) ActionType.Walk || bufferAction.actASC == (byte) ActionType.Sit)
            {
                tarIdx = 2;
                gap = 0.1f;
            }
        }

        List<float> gaps = new List<float>();
        for (int i = 0; i < pre.Count - 1; i++)
        {
            gaps.Add((pre[i + 1].GetPosition() - pre[i].GetPosition()).sqrMagnitude);
        }

        int si = 0;
        float tdist = float.PositiveInfinity;
        for (int i = taction.idxOptim; i < pre.Count - 1; i++)
        {
            float tgap = (root.GetPosition() - pre[i].GetPosition()).sqrMagnitude;
            if (tgap < tdist)
            {
                tdist = tgap;
                si = i;
            }
        }

        taction.idxOptim = si;

        int ei = si + step;
        if (ei >= pre.Count) ei = pre.Count - 1;
        Vector3 direct = (pre[ei].GetPosition() - pre[si].GetPosition()).normalized;
        float maxDist = Vector3.Distance(pre[si].GetPosition(), pre[pre.Count - 1].GetPosition());

        for (int i = 0; i < 6; i++)
        {
            float addDist = (i + 1) * gap;
            if (addDist > maxDist)
            {
                addDist = maxDist;
            }

            Vector3 npos = pre[si].GetPosition() + direct * addDist;
            // Vector3 npos = pre[si].GetPosition() + direct * (i + 1) * gap;
            Quaternion trot = Quaternion.identity;
            trot.SetFromToRotation(Vector3.forward, direct);
            Matrix4x4 p = Matrix4x4.TRS(npos, trot, pre[i].GetScale());
            res.Add(p);
        }

        if (taction.tarOptim == null)
        {
            taction.tarOptim = new List<Matrix4x4>();
            taction.tarOptim.Add(res[tarIdx]);
        }

        taction.tarOptim[0] = res[tarIdx];
        taction.hasTarget = false; // 按照指定路径行进时,设置目标为前向的点
        //taction.actASC = (byte)ActionType.Walk;

        if (drawTrajectory)
        {
            GameObject t = GameObject.CreatePrimitive(PrimitiveType.Sphere);
            t.transform.position = res[tarIdx].GetPosition();
            t.transform.localScale = new Vector3(0.2f, 0.2f, 0.2f);

            GameObject tr = GameObject.CreatePrimitive(PrimitiveType.Sphere);
            tr.transform.position = root.GetPosition();
            tr.transform.localScale = new Vector3(0.2f, 0.2f, 0.2f);
            Material m_tr = tr.GetComponent<Renderer>().material;
            m_tr.color = Color.blue;

            GameObject td = GameObject.CreatePrimitive(PrimitiveType.Sphere);
            td.transform.position = pre[si].GetPosition();
            td.transform.localScale = new Vector3(0.2f, 0.2f, 0.2f);
            Material m_material = td.GetComponent<Renderer>().material;
            m_material.color = Color.red;
        }

        float dist = (root.GetPosition() - pre[pre.Count - 1].GetPosition()).sqrMagnitude;
        if (dist < 0.1f)
        {
            Debug.Log("======>" + dist);
            //if (taction.actASC != taction.actASCBuffer) taction.actASC = taction.actASCBuffer;
            //taction.actASC = (byte)ActionType.Walk;
            taction.hasTarget = true;
            taction.routesOptim = null;
        }

        // Debug.Log(dist);
        return res;
    }

    public void Init()
    {
        info.getInfo();
        actionNum = info.actions.Count;
        action = info.actions[IdxAct];
        action.ResetStatus(transform.GetWorldMatrix()); // 输入 Anubis 的

        // Add new models 
        ModelsManager.AddModel(info);
        ModelCount = info.models.Count;

        ModelsManager.ClassifyModel(info.models);
    }


    // Start is called before the first frame update
    void Start()
    {
    }

    private IEnumerator waitActionBreak()
    {
        while (stayingActionBreak)
        {
            // 等待上一个动作的协程跳出来
            yield return new WaitForSeconds(0);
        }
    }

    // Update is called once per frame
    void Update()
    {
        if (IsChanged)
        {
            // getInfo 会重置之前的动作信息
            info.getInfo();
            actionNum = info.actions.Count;

            // Add models again if changed
            ModelsManager.AddModel(info);
            ModelCount = info.models.Count;

            IsChanged = false;


            // 每次新读入脚本，需要将 IdxAct 初始化为0
            IdxAct = 0;
            bufferAction = info.actions[IdxAct];
            Debug.Log("Input Action Num: " + info.actions.Count);
            Debug.Log("Process Action Line No: " + IdxAct);

            Debug.Log("Action Optim start.....................");
            // 对所有动作进行SA优化，此处优化第一个动作
            // 只处理 walk/run/sit
            isOptim = true;
            StartCoroutine(optimScene());
        }

        // When action over, continue next one if exists, or set idle
        if (!isOptim && action != null && action.isCompleted)
        {
            if ((IdxAct + 1) < actionNum)
            {
                // 上一个是 walk/run 要进行站住
                if (bufferAction.actASC == (byte) ActionType.Walk || bufferAction.actASC == (byte) ActionType.Run
                                                                  || bufferAction.actASC == (byte) ActionType.Idle)
                {
                    action = new ActionInfo();
                    action.ResetStatus(transform.GetWorldMatrix()); // 输入 Anubis 的
                }

                // 上一个是 sit 要进行停留
                if (bufferAction.actASC == (byte) ActionType.Sit)
                {
                    stayingActionBreak = true;
                    StartCoroutine(waitActionBreak());
                }

                IdxAct = IdxAct + 1;
                bufferAction = info.actions[IdxAct];
                Debug.Log("Process Action Line No: " + IdxAct);

                // 对所有动作进行SA优化，此处优化 next 动作
                // if (OptimizeOn)
                isOptim = true;

                StartCoroutine(optimScene());
            }
            else
            {
                action = new ActionInfo();
                action.ResetStatus(transform.GetWorldMatrix()); // 输入 Anubis 的
                Debug.Log("action null process end.....................");
            }
        }

        if (actOptimCompleted)
        {
            Debug.Log("Action process done !");
            actOptimCompleted = false;

            // 将优化后的解以及优化动作重新赋给 action+
            bufferAction.ResetStatus(this.transform.GetWorldMatrix());
            bufferAction.bothMotion = true; // 模型与Anubis一起运动

            info.actions[IdxAct] = bufferAction;
            action = bufferAction;

            isOptim = false;
            if (IdxAct == 0) IsChangedOptim = true;
            Debug.Log("Action update to NSM !");
            return;
        }

        if (isOptim && bufferAction != null && motionAvatar.GetComponent<NavMove>().isCompleted)
        {
            // 针对 walk / run
            // 程序 update navigation 获得具体路径
            // 结果保存在 motionAvatar.GetComponent<NavMove>().recPos;
            bufferAction.routesOptim = motionAvatar.GetComponent<NavMove>().recPos;

            // avg resample
            List<Matrix4x4> resample = new List<Matrix4x4>();
            int i = 0;
            while (i < bufferAction.routesOptim.Count - 1)
            {
                resample.Add(bufferAction.routesOptim[i]);

                int addIdx = 0;
                Vector3 direct =
                    (bufferAction.routesOptim[i + 1].GetPosition() - bufferAction.routesOptim[i].GetPosition())
                    .normalized;
                float dist = (bufferAction.routesOptim[i + 1].GetPosition() - bufferAction.routesOptim[i].GetPosition())
                    .sqrMagnitude;
                // Debug.Log(dist);

                if (dist < 0.005f)
                {
                    i += 1;
                    continue;
                }

                while (addIdx * 0.005f < dist)
                {
                    Vector3 tnpos = bufferAction.routesOptim[i].GetPosition() + direct * addIdx * 0.005f;
                    Quaternion trot = Quaternion.identity;
                    trot.SetFromToRotation(Vector3.forward, direct);
                    // Debug.Log(tnpos);
                    // Debug.Log(bufferAction.routesOptim[i].GetPosition());

                    // Matrix4x4 tp = Matrix4x4.TRS(tnpos, bufferAction.routesOptim[i+1].GetRotation(), bufferAction.routesOptim[i+1].GetScale());
                    Matrix4x4 tp = Matrix4x4.TRS(tnpos, trot, bufferAction.routesOptim[i + 1].GetScale());
                    resample.Add(tp);
                    addIdx += 1;
                }

                i += 1;
            }

            resample.Add(bufferAction.routesOptim[bufferAction.routesOptim.Count - 1]); // add last
            bufferAction.routesOptim = resample;
            bufferAction.routeLast = bufferAction.routesOptim[bufferAction.routesOptim.Count - 1];
            // Debug.Log(resample.Count);

            if (drawTrajectory) DerivedDraw();

            // walk & run 
            if (bufferAction.actASC == (byte) ActionType.Walk || bufferAction.actASC == (byte) ActionType.Run ||
                (!OptimizeOn && bufferAction.actASC == (byte) ActionType.Sit))
            {
                motionAvatar.GetComponent<NavMove>().ResetParams();

                actOptimCompleted = true;
                Debug.Log("Action Optim end [walk/run].....................");
                return;
            }

            if (OptimizeOn && bufferAction.actASC == (byte) ActionType.Sit)
            {
                motionAvatar.GetComponent<NavMove>().ResetParams();

                // sa 优化中，调用 navigation
                actNavCompleted = true;
                return;
            }

            Debug.Log("Navigation not support this action.");
            return;
        }

        if (isOptim && isSAOptim && action != null && action.isCompleted)
        {
            // 此时调用 NSM 按照预定轨迹完成动作已经完成
            actNSMCompleted = true;
            return;
        }
    }

    private void DerivedDraw()
    {
        if (bufferAction.routesOptim == null)
        {
            return;
        }

        Color[] colors = UltiDraw.GetRainbowColors(1);
        for (int i = 0; i < bufferAction.routesOptim.Count; i++)
        {
            Vector3 point = bufferAction.routesOptim[i].GetPosition();

            GameObject t = GameObject.CreatePrimitive(PrimitiveType.Sphere);
            t.transform.position = point;
            t.transform.localScale = new Vector3(0.1f, 0.1f, 0.1f);
            t.GetComponent<Renderer>().material.color = colors[0];
        }
    }

    public void setSmodelStatus(StatInfo oldS)
    {
        // // 获取场景中 Anubis 下面所有对象的transform，对应的是模型以及骨骼点的 transform
        // GameObject smodel = GameObject.Find("Anubis");
        // Transform[] father = smodel.GetComponentsInChildren<Transform>(); // 返回中包括 anubis 自己的 transform
        // // curS.smodel = new Dictionary<string, Matrix4x4>();

        // smodel.transform.position = oldS.nsm.GetPosition();
        // smodel.transform.rotation = oldS.nsm.GetRotation();
        // smodel.transform.localScale = oldS.nsm.GetScale(); // TODO 全局赋值局部，会不会有问题 

        // foreach (var child in father)
        // {
        //     child.transform.position = oldS.smodel[child.name].GetPosition();
        //     child.transform.rotation = oldS.smodel[child.name].GetRotation();
        //     child.transform.localScale = oldS.smodel[child.name].GetScale(); // TODO 全局赋值局部，会不会有问题         
        // }


        // 获取场景中 anim 下面所有对象的transform，对应的是模型的 transform
        GameObject smodel = motionAvatar.gameObject;
        Transform[] father = smodel.GetComponentsInChildren<Transform>(); // 返回中包括 anubis 自己的 transform
        // curS.smodel = new Dictionary<string, Matrix4x4>();

        // smodel.transform.position = oldS.nsm.GetPosition();
        // smodel.transform.rotation = oldS.nsm.GetRotation();
        // smodel.transform.localScale = oldS.nsm.GetScale(); // TODO 全局赋值局部，会不会有问题 

        foreach (var child in father)
        {
            //Debug.Log("===00===>"+child.name);
            child.transform.position = oldS.smodel[child.name].GetPosition();
            child.transform.rotation = oldS.smodel[child.name].GetRotation();
            child.transform.localScale = oldS.smodel[child.name].GetScale(); // TODO 全局赋值局部，会不会有问题         
        }
    }

    //*************************OptimScene********************************************************************

    private IEnumerator optimScene()
    {
        // action 按照不同动作进行优化: walk, run, sit
        // 针对 bufferAction 优化

        // 一定不优化的动作
        if (bufferAction.actASC == (byte) ActionType.Turn)
        {
            actOptimCompleted = true;
            Debug.Log("Action Optim end [turn].....................");
            yield return new WaitForSeconds(0f);
        }

        if (!OptimizeOn)
        {
            if (bufferAction.actASC == (byte) ActionType.Idle)
            {
                actOptimCompleted = true;
                Debug.Log("Action Optim end [idle].....................");
                yield return new WaitForSeconds(0f);
            }

            if (bufferAction.actASC == (byte) ActionType.Walk || bufferAction.actASC == (byte) ActionType.Run ||
                (!OptimizeOn && bufferAction.actASC == (byte) ActionType.Sit))
            {
                // 有目的地物体时,寻找最近的物体
                // 没有交互物体,即使有辅助动作物体,也不用寻路
                if (bufferAction.hasTarget)
                {
                    string tarName;
                    if (bufferAction.targets.Count > 1)
                    {
                        tarName = bufferAction.targets[1].modelName;
                    }
                    else
                    {
                        tarName = bufferAction.targets[0].modelName;
                    }

                    Interaction tarInt = ModelsManager.GetClosestModel(transform, tarName);
                    if (tarInt == null) yield return new WaitForSeconds(0f);
                    tarInterModel = tarInt.gameObject;

                    if (bufferAction.hasStyleObj)
                    {
                        string styName;
                        if (bufferAction.styleObjs.Count > 1)
                        {
                            styName = bufferAction.styleObjs[1].modelName;
                        }
                        else
                        {
                            styName = bufferAction.styleObjs[0].modelName;
                        }

                        // 对于手上直接出现的模型，直接跳过
                        string[] interObjs = {"phone", "games"};
                        if (!interObjs.Contains(styName))
                        {
                            Interaction styInt = ModelsManager.GetClosestModel(transform, styName);
                            if (styInt == null) yield return new WaitForSeconds(0f);
                            tarStyleModel = styInt.gameObject;
                        }
                    }

                    // 调用 navigation 找到最佳路径然后返回
                    bufferAction.bothMotion = false;
                    navSearchRoutine();
                }

                // 对于没有目标物体的 walk/run 直接返回
                yield return new WaitForSeconds(0f);
            }
        }

        if (OptimizeOn)
        {
            if (bufferAction.actASC == (byte) ActionType.Idle)
            {
                if (bufferAction.style == null)
                {
                    // stand & 没有附属动作，直接跳过
                    Debug.Log("Not found style action.");
                    yield return new WaitForSeconds(0f);
                }
                else
                {
                    tarInterModel = null; //直接设置 stand 的交互物体为 null
                    if (bufferAction.hasStyleObj)
                    {
                        string styName;
                        if (bufferAction.styleObjs.Count > 1)
                        {
                            styName = bufferAction.styleObjs[1].modelName;
                        }
                        else
                        {
                            styName = bufferAction.styleObjs[0].modelName;
                        }

                        Interaction styInt = ModelsManager.GetClosestModel(transform, styName);
                        if (styInt == null) yield return new WaitForSeconds(0f);
                        tarStyleModel = styInt.gameObject;

                        Debug.Log("Style Action Interaction Model Name: " + tarStyleModel.name);
                    }

                    StartCoroutine(RunOptim());

                    while (!isSAOptim)
                    {
                        yield return new WaitForSeconds(0f);
                    }
                }
            }

            if (bufferAction.actASC == (byte) ActionType.Sit)
            {
                if (!bufferAction.hasTarget)
                {
                    // 必须要指定目标物体，如果由临近物体执行，将非常难以优化
                    Debug.Log("Not found interaction model.");
                    yield return new WaitForSeconds(0f);
                }
                else
                {
                    // 调用 optimation 找到最佳路径以及物体布局
                    string tarName;
                    if (bufferAction.targets.Count > 1)
                    {
                        tarName = bufferAction.targets[1].modelName;
                    }
                    else
                    {
                        tarName = bufferAction.targets[0].modelName;
                    }

                    Interaction tarInt = ModelsManager.GetClosestModel(transform, tarName);
                    if (tarInt == null) yield return new WaitForSeconds(0f);
                    tarInterModel = tarInt.gameObject;

                    if (bufferAction.hasStyleObj)
                    {
                        string styName;
                        if (bufferAction.styleObjs.Count > 1)
                        {
                            styName = bufferAction.styleObjs[1].modelName;
                        }
                        else
                        {
                            styName = bufferAction.styleObjs[0].modelName;
                        }

                        Interaction styInt = ModelsManager.GetClosestModel(transform, styName);
                        if (styInt == null) yield return new WaitForSeconds(0f);
                        tarStyleModel = styInt.gameObject;
                    }

                    Debug.Log("Interaction Model Name: " + tarInterModel.name);
                    StartCoroutine(RunOptim());

                    while (!isSAOptim)
                    {
                        yield return new WaitForSeconds(0f);
                    }
                }
            }
        }
    }

    public void navSearchRoutine()
    {
        if (tarInterModel == null)
        {
            // 注意必须设置其为 true, 使得后续优化可以进行
            actNavCompleted = true;
            return;
        }

        // // 调用 navigation 找到最佳路径然后返回
        // Interaction tarInt = ModelsManager.GetClosestModel(transform, tarModName);
        // if (tarInt == null)
        // {
        //     Debug.Log("Not found interaction model !");
        //     // TODO 没有处理异常
        // }
        // GameObject targetObj = tarInt.gameObject;
        Transform[] father = tarInterModel.GetComponentsInChildren<Transform>();

        // if (targetObj.name != tarModName)
        // {
        //     Debug.Log("Found interaction model do not match the input !");
        // }

        // Transform[] father = GameObject.Find(tarModName).GetComponentsInChildren<Transform>();
        foreach (var child in father)
        {
            if (child.name == "Rfoot")
            {
                motionAvatar.GetComponent<NavMove>().ResetParams();
                motionAvatar.GetComponent<NavMove>().target = child.transform;
                // Debug.Log(motionAvatar.GetComponent<NavMove>().target.position);
                // Debug.Log(child.name);
                //return child.transform;
                return;
            }
        }

        Debug.Log("Interaction model do not haved Rfoot.");

        motionAvatar.GetComponent<NavMove>().ResetParams();
        motionAvatar.GetComponent<NavMove>().target = tarInterModel.transform;
        //return motionAvatar.GetComponent<NavMove>().target;
        return;
    }


    public Transform haveRfoot()
    {
        Transform[] father = tarInterModel.GetComponentsInChildren<Transform>();

        // if (targetObj.name != tarModName)
        // {
        //     Debug.Log("Found interaction model do not match the input !");
        // }

        // Transform[] father = GameObject.Find(tarModName).GetComponentsInChildren<Transform>();
        foreach (var child in father)
        {
            if (child.name == "Rfoot")
            {
                motionAvatar.GetComponent<NavMove>().ResetParams();
                motionAvatar.GetComponent<NavMove>().target = child.transform;
                // Debug.Log(motionAvatar.GetComponent<NavMove>().target.position);
                // Debug.Log(child.name);
                return child.transform;
            }
        }

        return null;
    }


    //*************************Heuristic Optim********************************************************************
    private float thresholdOptim1 = 0.5f, thresholdOptim2 = 0.5f;

    private int maxEpoches = 20;
    //public IEnumerator RunOptimHeuristic()
    //{
    //    // 用启发式的方法去找解，而非随机扰动
    //    // 1.先调整椅子
    //    // 2.再调整stykeobj
    //    // 迭代，当Loss小于threshold停止

    //    isSAOptim = true;
    //    OptimLog($"\nOptimize Begin: {DateTime.Now}");
    //    OptimLog($"threshold1:{thresholdOptim1},threshold2:{thresholdOptim2}");

    //    bufferOld = new StatInfo();  // 初始化优化状态
    //    bufferCur = new StatInfo();  // 初始化优化状态
    //    optimOld = new StatInfo();  // 初始化优化状态
    //    optimCur = new StatInfo();  // 初始化优化状态
    //    bufferOld = getCurStatus(); // 优化前获取目前状态

    //    //**********************************************************************
    //    // 使用 nav 获取最优路径; 由于每一次寻路结果并不相同,所以只运行一次
    //    bufferAction.bothMotion = false;
    //    navSearchRoutine();
    //    while (!actNavCompleted)
    //    {
    //        // // update 中对路径进行 resample 后设置 actNavCompleted = true
    //        // // bufferAction.routesOptim 存储最终路径
    //        yield return new WaitForSeconds(0f);
    //    }
    //    actNavCompleted = false;
    //    bufferAction.bothMotion = true;
    //    // 人物模型的位置还原
    //    motionAvatar.transform.position = bufferOld.model.GetPosition();
    //    motionAvatar.transform.rotation = bufferOld.model.GetRotation();

    //    bufferOld.navRoutes = new List<Matrix4x4>();
    //    for (int i = 0; i < bufferAction.routesOptim.Count; i++)
    //    {
    //        bufferOld.navRoutes.Add(bufferAction.routesOptim[i]);
    //    }
    //    Debug.Log("Optim Nav search have done !");
    //    //**********************************************************************

    //    bestErrorFound = float.PositiveInfinity;
    //    float oldError = float.PositiveInfinity;

    //    // // 依据目标动作以及交互物体，获取初解
    //    // // 初解并不添加扰动
    //    optimCur = getCurStatus();
    //    StartCoroutine(Mutate(false)); // 更新 optimOld
    //    while (!perOptimCompleted)
    //    {
    //        yield return new WaitForSeconds(0f);
    //    }
    //    perOptimCompleted = false;

    //    optimOld = optimCur;  // mutate 中更新的是 cur，但是这里计算的是初解，所以赋值old; 注意此处要进行值复制

    //    float loss1 = 0;
    //    Vector3 loss2 = new Vector3();
    //    int passport = 0;
    //    for (int i = 0; i < maxEpoches; i++)
    //    {
    //        Debug.Log("Optim epoch Num:" + i);

    //        // 调整椅子，直到穿模减少
    //        optimCur = getCurStatus();
    //        StartCoroutine(Mutate(false));
    //        while (!perOptimCompleted)
    //        {
    //            yield return new WaitForSeconds(0f);
    //        }
    //        perOptimCompleted = false;

    //        loss1 = GetTargetLoss(optimCur);

    //        while (loss1 > thresholdOptim1)
    //        {
    //            AdjustTarget(optimCur, loss1);

    //            optimCur = getCurStatus();
    //            StartCoroutine(Mutate(false));
    //            while (!perOptimCompleted)
    //            {
    //                yield return new WaitForSeconds(0f);
    //            }
    //            perOptimCompleted = false;

    //            loss1 = GetTargetLoss(optimCur);
    //        }

    //        // 调整styleObj,一次到位
    //        loss2 = GetStyleObjDistance();
    //        AdjustStyleTarget(optimCur, loss2);


    //        optimCur = getCurStatus();
    //        StartCoroutine(Mutate(false));
    //        while (!perOptimCompleted)
    //        {
    //            yield return new WaitForSeconds(0f);
    //        }
    //        perOptimCompleted = false;
    //        loss1 = GetTargetLoss(optimCur);
    //        loss2 = GetStyleObjDistance();
    //        OptimLog($"loss1: {loss1:F5} loss2: {loss2:F5}");

    //        // 连续三次达到阈值，即可退出迭代
    //        if (loss1 < thresholdOptim1 + 0.001f && loss2.sqrMagnitude < thresholdOptim2)
    //        {
    //            passport++;
    //            if (passport >= 3) break;
    //        }
    //        else passport = 0;
    //        // 若无法达到，可以记录最小值
    //    }
    //    OptimLog($"H Optimize End: loss1: {loss1:F5} loss2: {loss2:F5}");


    //    // 返回最终的状态，且数据保存在 buffCur 中
    //    bufferCur = getCurStatus();

    //    Debug.Log("H optim have done !");
    //    // 最终结果由 bufferCur 到 bufferAction

    //    //resetModels(bufferCur);
    //    // 优化过程中,调用NSM 不知道那里将 routesOptim 置为null,下面复原一下
    //    if (bufferAction.routesOptim == null)
    //    {
    //        bufferAction.routesOptim = new List<Matrix4x4>();
    //        for (int i = 0; i < bufferOld.navRoutes.Count; i++)
    //        {
    //            bufferAction.routesOptim.Add(bufferOld.navRoutes[i]);
    //        }
    //    }

    //    Debug.Log("Action Optim end [sit].....................");

    //    // 迭代优化结束
    //    isSAOptim = false;
    //    actOptimCompleted = true;
    //}

    float GetTargetLoss(StatInfo sta)
    {
        float costD = 0;

        foreach (var target in sta.targets)
        {
            var model = info.models[target].model;
            if (sta.collisionDepth.ContainsKey(model))
            {
                var tmp = sta.collisionDepth[model];
                costD += 1.0f * tmp.Item1.Count / tmp.Item2;
            }
        }

        OptimLog($"target loss:{costD}");

        return costD;
    }

    private float adjustTargetCoef = 0.1f;

    void AdjustTarget(StatInfo sta, float loss1)
    {
        foreach (var target in sta.targets)
        {
            var model = info.models[target].model;

            // 调整的方法是向后移动0.1f
            model.transform.position -= model.transform.forward * adjustTargetCoef;
        }
    }


    private string targetBone = "LMidHand";

    Vector3 GetStyleObjDistance()
    {
        // 只获取一个点的距离

        //Dictionary<string, Vector3> point_adopted = new Dictionary<string, Vector3>(getLowest());
        Dictionary<string, Vector3> point_adopted = new Dictionary<string, Vector3>(getAverage());

        Vector3 cost = new Vector3(0, 0, 0);
        Dictionary<string, string> boneMaps_hasStyleObj = new Dictionary<string, string>
            {{"LMidHand", "LeftMiddleDistal"}, {"RMidHand", "RightMiddleDistal"}};
        Dictionary<string, string> boneMaps_nonStyleObj = new Dictionary<string, string>
            {{"RightWrist", "LeftHand"}, {"LeftWrist", "RightHand"}};

        if (bufferAction.hasStyleObj)
        {
            Transform[] father = tarStyleModel.GetComponentsInChildren<Transform>();
            foreach (var child in father)
            {
                if (child.name == targetBone && boneMaps_hasStyleObj.ContainsKey(child.name) &&
                    point_adopted.ContainsKey(boneMaps_hasStyleObj[child.name]))
                {
                    Vector3 tbMod = point_adopted[boneMaps_hasStyleObj[child.name]];
                    //Vector3 tbMod = optimCur.lastOptimFrame[boneMaps_hasStyleObj[child.name]].GetPosition();
                    //costC += 10 * Vector3.Distance(child.transform.position, tbMod);

                    cost = tbMod - child.transform.position;

                    drawPoint(tbMod, Color.red);
                }
            }
        }

        // ********************************************
        // 迭代完成重置
        PosRecord = new List<Dictionary<string, Vector3>>();
        // *********************************************
        return cost;
    }

    void AdjustStyleTarget(StatInfo sta, Vector3 distance)
    {
        foreach (var target in sta.styleTargets)
        {
            var model = info.models[target].model;

            // 调整的方法是瞬移
            model.transform.position += distance;
        }

        // 对其他相关物体同步位移
        foreach (var obj in sta.styleTargetObj.Keys)
            if (obj != sta.styleTargets[0])
            {
                var model = info.models[obj].model;
                model.transform.position += distance;
            }
    }
    //************************SA Optim****************************************************************************************

    private void resetModels(StatInfo s)
    {
        foreach (var tmp in s.objTrans)
        {
            var trans = info.models[tmp.Key].model.transform;
            trans.position = tmp.Value.GetPosition();
            trans.localScale = tmp.Value.GetScale();
            trans.rotation = tmp.Value.GetRotation();
        }
    }

    private float AcceptanceProbability(float oldError, float newError, float temperature)
    {
        return (float) Math.Exp((oldError - newError) / temperature); // -1 * (new - old)
    }

    [Header("Drop Options")]
    public float temperature = 1f;
    public float coolingFactor = .98f;

    [Header("Self Options")]
    public int epochs = 150;

    // 模拟退火的参数
    // int epochs = 1400000;
    

    public IEnumerator RunOptim()
    {
        // 迭代优化开始
        isSAOptim = true;
        OptimLog($"\nOptimize Begin: {DateTime.Now}");

        bufferOld = new StatInfo(); // 初始化优化状态
        bufferCur = new StatInfo(); // 初始化优化状态
        optimOld = new StatInfo(); // 初始化优化状态
        optimCur = new StatInfo(); // 初始化优化状态
        bufferOld = getCurStatus(); // 优化前获取目前状态
        motionAvatar.GetComponent<NavMove>().SetNav();

        //**********************************************************************
        if (bufferAction.hasTarget)
        {
            // 使用 nav 获取最优路径; 由于每一次寻路结果并不相同,所以只运行一次
            bufferAction.bothMotion = false;
            navSearchRoutine();
            while (!actNavCompleted)
            {
                // // update 中对路径进行 resample 后设置 actNavCompleted = true
                // // bufferAction.routesOptim 存储最终路径
                yield return new WaitForSeconds(0f);
            }

            actNavCompleted = false;
            bufferAction.bothMotion = true;
            // 将模型的位置还原
            motionAvatar.transform.position = bufferOld.model.GetPosition();
            motionAvatar.transform.rotation = bufferOld.model.GetRotation();

            bufferOld.navRoutes = new List<Matrix4x4>();
            for (int i = 0; i < bufferAction.routesOptim.Count; i++)
            {
                bufferOld.navRoutes.Add(bufferAction.routesOptim[i]);
            }

            motionAvatar.GetComponent<NavMove>().ClearNav();
            Debug.Log("Optim Nav search have done !");
        }

        //**********************************************************************

        float error = float.PositiveInfinity;
        bestErrorFound = float.PositiveInfinity;
        float oldError = float.PositiveInfinity;

        // // 依据目标动作以及交互物体，获取初解
        // // 初解并不添加扰动
        optimCur = new StatInfo(); // 初始化优化状态
        optimCur = getCurStatus();
        StartCoroutine(Mutate(false)); // 更新 optimOld

        while (!perOptimCompleted)
        {
            yield return new WaitForSeconds(0f);
        }

        perOptimCompleted = false;
        //Debug.Log("==========>detect 4");

        optimOld = optimCur; // mutate 中更新的是 cur，但是这里计算的是初解，所以赋值old; 注意此处要进行值复制

        // 模拟退火的参数
        // int epochs = 1400000;
        //float temperature = 1f;
        //float coolingFactor = .98f;

        // 记录各个loss值
        Vector3 loss = new Vector3();
        Vector3 oldloss = new Vector3();

        OptimLog($"Epochs:{epochs}, ColFac:{coolingFactor},TargetPos:{targetPos},TargetScale:{targetScl}," +
                 $"ScalePos:{StylePos},StyleScale:{StyleScl},lambda:{lambdaD} {lambdaR} {lambdaC},");

        for (int i = 0; i < epochs; i++)
        {
            Debug.Log("Optim epoch Num:" + i);
            temperature *= coolingFactor;
            optimCur = new StatInfo(); // 初始化优化状态
            optimCur = getCurStatus();
            StartCoroutine(Mutate(true));
            // mutate 中更新的是 cur
            while (!perOptimCompleted)
            {
                yield return new WaitForSeconds(0f);
            }

            perOptimCompleted = false;

            error = CalculateError(out loss);
            if (i == 0)
                oldError = error; //第一次迭代先赋值error
            float ap = AcceptanceProbability(oldError, error, temperature);
            if (AcceptanceProbability(oldError, error, temperature) > (float) seed.NextDouble()) // access solution
            {
                // 统计概率 seed.NextDouble() 长期概率是0.5
                oldError = error;
                oldloss = loss;
            }
            else if (error < oldError) // access solution
            {
                oldError = error;
                oldloss = loss;
            }
            else // not access solution
            {
                resetModels(optimCur);
            }

            if (error < bestErrorFound)
            {
                bestErrorFound = error;
                optimCur = getCurStatus();
                optimOld = optimCur;
            }

            // Console.WriteLine($"old error: {oldError} error: {error} temperature: {temperature}");
            OptimLog($"old error: {oldError} error: {error} bestError: {bestErrorFound} temperature: {temperature} acccess: {ap} Direct:{BonePointDirect}");
            OptimLog($"loss1: {oldloss.x}  loss2: {oldloss.y}  loss3: {oldloss.z}");
        }

        OptimLog($"Optimize End: {DateTime.Now}, bestError:{bestErrorFound}");

        // 返回最终的状态，且数据保存在 buffCur 中
        bufferCur = optimOld;

        Debug.Log("SA optim have done !");
        // 最终结果由 bufferCur 到 bufferAction

        resetModels(bufferCur);
        // 优化过程中,调用NSM 不知道那里将 routesOptim 置为null,下面复原一下
        if (bufferAction.hasTarget)
        {
            if (bufferAction.routesOptim == null)
            {
                bufferAction.routesOptim = new List<Matrix4x4>();
                for (int i = 0; i < bufferOld.navRoutes.Count; i++)
                {
                    bufferAction.routesOptim.Add(bufferOld.navRoutes[i]);
                }
            }
        }

        Debug.Log("Action Optim end [sit].....................");

        // 迭代优化结束
        isSAOptim = false;
        actOptimCompleted = true;
    }

    private IEnumerator Mutate(bool addDisturb)
    {
        // 获取新解：
        // 使用 nav 获取最优路径
        // 调用 NSM 获取人物最后的动作状态
        // 记录角色执行动作时：
        //     1.开始到结束有无碰撞物体；
        //     2.动作执行过程中，是否满足一定阈值内与交互物体的接触；
        //     3.人物动作符合骨骼运动 IK
        // 存储下过程状态

        // 改变交互物体以及其连接物体，人体的骨骼改变量
        if (addDisturb) setDisturbSolve();

        // 优化前设置参数        
        bufferAction.bothMotion = true; // 模型与Anubis一起运动
        bufferAction.ResetStatus(transform.GetWorldMatrix()); // 输入 Anubis 的
        if (bufferAction.hasTarget)
        {
            if (bufferAction.routesOptim == null)
            {
                bufferAction.routesOptim = new List<Matrix4x4>();
                for (int i = 0; i < bufferOld.navRoutes.Count; i++)
                {
                    bufferAction.routesOptim.Add(bufferOld.navRoutes[i]);
                }
            }
        }
        action = bufferAction; // 更新action，使得 sig 运动

        // 注意:此时应该在 NSM 运行的过程中记录好:1.碰撞的物体;2.运动轨迹;3.其他需要的信息
        // 并且更新到 optimCur.collisionDepth, 注意每一次更新都要重置
        // nsmOptimRoutes 记录的整个过程中人物轨迹
        optimCur.collisionDepth = new Dictionary<GameObject, (HashSet<BoxCollider>, int)>();
        optimCur.nsmOptimRoutes = new List<Matrix4x4>();

        // 等待 sig 执行完成
        isNSMOptim = true;
        while (!actNSMCompleted)
        {
            // sig 已经执行完成，并且已经记录执行过程中的状态信息
            yield return new WaitForSeconds(0f);
        }

        actNSMCompleted = false;
        isNSMOptim = false;
        Debug.Log("Optim NSM process have done !");

        // NSM 优化完之后,记录当前模型骨骼的 transform
        optimCur.lastOptimFrame = getAnimTransforms();


        // 重置到之前的动作状态
        if (IdxAct - 1 < 0)
        {
            action = new ActionInfo();
        }
        else
        {
            action = this.info.actions[IdxAct - 1];
        }

        isResetOptim = true;
        while (!actResetCompleted)
        {
            yield return new WaitForSeconds(0f);
        }

        actResetCompleted = false;
        isResetOptim = false;

        motionAvatar.GetComponent<NavMove>().ResetParams();

        perOptimCompleted = true;
    }

    // **********************Disturb****************************
    System.Random rand = new System.Random(unchecked((int) DateTime.Now.Ticks)); //reuse this if you are generating many

    private double GaussRandom(double mean = 0, double stdDev = 1)
    {
        // GaussRandom in [mean - 3 * stdDev, mean + 3 * stdDev]
        double u1 = 1.0 - rand.NextDouble(); //uniform(0,1] random doubles
        double u2 = 1.0 - rand.NextDouble();
        double randStdNormal = Math.Sqrt(-2.0 * Math.Log(u1)) *
                               Math.Sin(2.0 * Math.PI * u2); //random normal(0,1)

        if (randStdNormal <= -3) randStdNormal = -3;
        if (randStdNormal >= 3) randStdNormal = 3;

        double randNormal = mean + stdDev * randStdNormal; //random normal(mean,stdDev^2)
        return randNormal;
    }

    private Vector3 GaussRandumVector3(double mean = 0, double stdDev = 1)
    {
        return new Vector3((float) GaussRandom(mean, stdDev), (float) GaussRandom(mean, stdDev),
            (float) GaussRandom(mean, stdDev));
    }


    public float targetPos = 0.01f, targetScl = 0.01f;
    public float StylePos = 0.01f, StyleScl = 0.01f;


    public float posibiltyDirec = 0.7f;
    private void setDisturbSolve()
    {
        // 1.bones: leftHead, rightHead, Hip  pos, Rx   ; foot dont change   

        // 这部分摊子铺的太大。一开始不应该选取那么多变量作为扰动，解空间巨大迭代次数又有限，很难找到合适的解
        // 就只优化一个位置。先能得到一个优化的结果。其实其他变量对效果的影响并不大。

        // 2.objects: scale, pos
        // bufferOld.objTrans has got
        // 默认所有相关物体的position都在中间，且都要有Collider和Bounds

        foreach (var i in bufferOld.targets)
        {
            Transform trans = info.models[i].model.transform;

            Vector3 posD = GaussRandumVector3(0, targetPos);
            Vector3 sclD = new Vector3(1, 1, 1) + GaussRandumVector3(0, targetScl);

            posD.y = 0;
            sclD.x = 1;
            sclD.z = 1;


            var realFather = trans.parent;
            var fakeFather = new GameObject("fakeParent").transform;
            trans.parent = fakeFather;
            fakeFather.localScale = Vector3.Scale(fakeFather.localScale, sclD);
            fakeFather.position += posD;
            trans.position += posD;
            trans.parent = realFather;
            Destroy(fakeFather.gameObject);

            OptimLog(string.Format("{0}: position + {1:F5}  scale * {2:F5}", info.models[i].modelType, posD, sclD));
        }

        foreach (var i in bufferOld.styleTargets)
        {
            Transform trans = info.models[i].model.transform;

            Vector3 posD = GaussRandumVector3(0, StylePos);
            Vector3 sclD = new Vector3(1, 1, 1);
            Vector3 sclRan = GaussRandumVector3(0, StyleScl);

            // 启发式
            // 每个方向都有0.7的概率往接触点移动
            if ( (1-posibiltyDirec) < (float)seed.NextDouble())
            {
                sclD.y += BonePointDirect.y > 0 ? Mathf.Abs(sclRan.y) : -Mathf.Abs(sclRan.y);
            }
            else
                sclD.y +=  sclRan.y;

            if ((1 - posibiltyDirec) < (float)seed.NextDouble())
            {
                posD.x = BonePointDirect.x > 0 ? Mathf.Abs(posD.x) : -Mathf.Abs(posD.x);
            }
            if ((1 - posibiltyDirec) < (float)seed.NextDouble())
            {
                posD.z = BonePointDirect.z > 0 ? Mathf.Abs(posD.z) : -Mathf.Abs(posD.z);
            }

            Vector3 size = trans.GetComponent<VoxelCollider>().GetSize();


            // x,z调整容易出问题
            sclD.x = 1;
            sclD.z = 1;
            posD.y = 0;

            //贴地的要保持

            // 先对styleObj扰动
            //trans.position += posD;
            //trans.localScale = Vector3.Scale(trans.localScale, sclD);
            OptimLog(string.Format("{0}: position + {1:F5}  scale * {2:F5}", info.models[i].modelType, posD, sclD));

            var realFather = trans.parent;
            var fakeFather = new GameObject("fakeParent").transform;
            foreach (var j in bufferOld.styleTargetObj.Keys)
            {
                var model = info.models[j].model.transform;
                model.parent = fakeFather;
            }


            fakeFather.localScale = Vector3.Scale(fakeFather.localScale, sclD);
            fakeFather.position += posD;
            foreach (var j in bufferOld.styleTargetObj.Keys)
            {
                var model = info.models[j].model.transform;
                model.parent = realFather;
            }

            Destroy(fakeFather.gameObject);

            // 对styleObj进行一个增加的位置扰动，调整相对的位置
            //Vector3 posDr = GaussRandumVector3(0, StylePos);
            //posDr.y = 0;
            //trans.position += posDr;

            return;
            // 根据StyleObj的扰动调整关联物体，关联物体都要有Collider和Bounds
            // 这部分计算很有问题。但是关键是StyleObj的移动，其他不是关键，所以直接简单平移
            int rt = bufferOld.styleTargetObj[i];
            if (rt != -1)
            {
                // 先调整底座, 约束是不高于物体底面且保持贴地，posX,posZ变化等同。ScaleX,Z不调
                Transform transRt = info.models[rt].model.transform;
                //Vector3 sizeRt = transRt.GetComponent<VoxelCollider>().GetSize();

                Vector3 posDRt = posD, sclDRt = new Vector3(1, 1, 1);
                float l = posD.y - size.y * (sclD.y - 1) * 0.5F;
                //sclDRt.y = (sizeRt.y - l) / sizeRt.y;
                posDRt.y = l / 2;

                transRt.position += posD;
                //transRt.localScale = Vector3.Scale(transRt.localScale, sclDRt);
                OptimLog(string.Format("{0}: position + {1:F5}  scale * {2:F5}", info.models[rt].modelType, posDRt,
                    sclDRt));


                // 再调整底座上的物体，约束是保持贴合桌面，posX,posZ变化等同。Scale不调。
                foreach (var kp in bufferOld.styleTargetObj)
                {
                    if (kp.Value == rt && kp.Key != i)
                    {
                        int j = kp.Key;

                        Transform transJ = info.models[j].model.transform;
                        //Vector3 sizeJ = transJ.GetComponent<VoxelCollider>().GetSize();


                        Vector3 posDJ = posD, sclDJ = new Vector3(1, 1, 1);
                        posDJ.y = l;

                        transJ.position += posD;
                        //transJ.localScale = Vector3.Scale(transJ.localScale, sclDJ);
                        OptimLog(string.Format("{0}: position + {1:F5}  scale * {2:F5}", info.models[j].modelType,
                            posDJ, sclDJ));
                    }

                    // todo pos不一定在正中间，size可能不准确。计算上下平面坐标时可能误差。
                }
            }
        }
    }


    // **********************Get StatInfo****************************
    public StatInfo getCurStatus()
    {
        //public Actor actor;  // Bone[] Bones 存储了骨骼点的信息
        //   public Dictionary<string, Transform> bones;  // Bone[] Bones 存储了骨骼点的信息
        //   public List<Matrix4x4> routes;

        //   // 交互以及相连物体的信息
        //   public List<GameObject> targetObj;
        //   public List<string> targets;
        //   public List<string> styleTargets;
        //   public Dictionary<GameObject, Matrix4x4> objTrans;

        //   //public Dictionary<string, Matrix4x4> allObjTrans;  //场景内所有物体的位置

        //   public Transform model;  // 人物模型的位置
        //   public Transform nsm;  // Anubis的位置


        StatInfo curS = new StatInfo();

        // curS.actor = GameObject.Find("Anubis").GetComponent<SIGGRAPH_Asia_2019>().Actor;
        curS.bones = motionAvatar.GetComponent<BVHDriver>().getAllBones();

        curS.routes = motionAvatar.GetComponent<NavMove>().recPos;
        curS.model = Matrix4x4.TRS(motionAvatar.transform.position, motionAvatar.transform.rotation,
            motionAvatar.transform.localScale);
        curS.nsm = Matrix4x4.TRS(this.transform.position, this.transform.rotation, this.transform.localScale);

        /* Get targets and targetObjs from : bufferAction */

        // 这里应该是全局变量?一个动作的target应该是固定的，不应该反复执行。
        curS.action = bufferAction;
        curS.targets = new List<int>();
        curS.styleTargets = new List<int>();
        curS.targetObj = new Dictionary<int, int>();
        curS.styleTargetObj = new Dictionary<int, int>();
        curS.objTrans = new Dictionary<int, Matrix4x4>();

        // find closet targets and styleTargets
        if (bufferAction.hasTarget)
        {
            foreach (var target in bufferAction.targets)
            {
                Interaction tarInt = ModelsManager.GetClosestModel(transform, target.modelName);

                if (tarInt == null) continue;

                GameObject targetObj = tarInt.gameObject;

                // only the object in ssg will be counted
                for (int i = 0; i < info.models.Count; ++i)
                {
                    if (info.models[i].model == targetObj)
                    {
                        curS.targets.Add(i);

                        var tmp = Scene.Instance.getRelatedModels(i);
                        foreach (var tmpt in tmp)
                        {
                            curS.targetObj[tmpt.Key] = tmpt.Value;
                        }

                        break;
                    }
                }
            }
        }

        if (bufferAction.hasStyleObj)
        {
            foreach (var styleTarget in bufferAction.styleObjs)
            {
                Interaction styleTarInt = ModelsManager.GetClosestModel(transform, styleTarget.modelName);

                if (styleTarInt == null) continue;

                GameObject styleTargetObj = styleTarInt.gameObject;


                for (int i = 0; i < info.models.Count; ++i)
                {
                    if (info.models[i].model == styleTargetObj)
                    {
                        curS.styleTargets.Add(i);

                        var tmp = Scene.Instance.getRelatedModels(i);
                        foreach (var tmpt in tmp)
                        {
                            curS.styleTargetObj[tmpt.Key] = tmpt.Value;
                        }

                        break;
                    }
                }
            }
        }

        // get related obj trans
        foreach (var v in curS.targetObj)
        {
            if (!curS.objTrans.ContainsKey(v.Key))
            {
                Transform child = info.models[v.Key].model.transform;
                curS.objTrans[v.Key] = Matrix4x4.TRS(child.position, child.rotation, child.localScale);
            }
        }

        foreach (var v in curS.styleTargetObj)
        {
            if (!curS.objTrans.ContainsKey(v.Key))
            {
                Transform child = info.models[v.Key].model.transform;
                curS.objTrans[v.Key] = Matrix4x4.TRS(child.position, child.rotation, child.localScale);
            }
        }


        /*
        // update allObjTrans
        Interaction[] models = GameObject.FindObjectsOfType<Interaction>();
        for (int i = 0; i < models.Length; i++)
        {
            string name = models[i].name;
            if (name.Contains("(Clone)"))
                name = name.Replace("(Clone)", string.Empty);

            Transform child = models[i].transform;
            curS.objTrans[models[i].name] = Matrix4x4.TRS(child.position, child.rotation, child.localScale);
        }
        */


        // 获取场景中 Anubis 下面所有对象的transform，对应的是模型以及骨骼点的 transform
        GameObject smodel = GameObject.Find("Anubis");
        Transform[] father = smodel.GetComponentsInChildren<Transform>(); // 返回中包括 anubis 自己的 transform
        curS.smodel = new Dictionary<string, Matrix4x4>();

        foreach (var child in father)
        {
            curS.smodel.Add(child.name, child.transform.GetWorldMatrix());
            // Debug.Log(child.name);
            // Debug.Log(child.transform.GetWorldMatrix().GetPosition());
        }

        // 记录该时刻,输入到NSM的输入信息
        curS.snsmInfo = GameObject.Find("Anubis").GetComponent<SIGGRAPH_Asia_2019>()._getCurStatus();

        return curS;
    }

    public StatInfo updateStatus(StatInfo oldS)
    {
        Debug.Log("walk update........");
        // 针对 walk / run 的路径更新

        StatInfo newS = new StatInfo(oldS);

        //curS.actor = GameObject.Find("Anubis").GetComponent<SIGGRAPH_Asia_2019>().Actor;

        newS.routes = motionAvatar.GetComponent<NavMove>().recPos;
        newS.bones = motionAvatar.GetComponent<BVHDriver>().getAllBones();
        newS.model = Matrix4x4.TRS(motionAvatar.transform.position, motionAvatar.transform.rotation,
            motionAvatar.transform.localScale);
        newS.nsm = Matrix4x4.TRS(this.transform.position, this.transform.rotation, this.transform.localScale);

        // update AllobjTrans
        /*
        Interaction[] models = GameObject.FindObjectsOfType<Interaction>();
        for (int i = 0; i < models.Length; i++)
        {
            string name = models[i].name;
            if (name.Contains("(Clone)"))
                name = name.Replace("(Clone)", string.Empty);

            Transform child = models[i].transform;
            newS.objTrans[models[i].name] = Matrix4x4.TRS(child.position, child.rotation, child.localScale);
        }
        */

        // 获取场景中 Anubis 下面所有对象的transform，对应的是模型以及骨骼点的 transform
        GameObject smodel = GameObject.Find("Anubis");
        Transform[] father = smodel.GetComponentsInChildren<Transform>(); // 返回中包括 anubis 自己的 transform
        oldS.smodel = new Dictionary<string, Matrix4x4>();

        foreach (var child in father)
        {
            oldS.smodel.Add(child.name, child.transform.GetWorldMatrix());
            // Debug.Log(child.name);
            // Debug.Log(child.transform.GetWorldMatrix().GetPosition());
        }

        return newS;
    }

    private Dictionary<string, Matrix4x4> getAnimTransforms()
    {
        Dictionary<string, Matrix4x4> trans = new Dictionary<string, Matrix4x4>();

        // 获取场景中 anim 下面所有对象的transform，对应的是模型的 transform
        GameObject smodel = motionAvatar.gameObject;
        Transform[] father = smodel.GetComponentsInChildren<Transform>(); // 返回中包括 anubis 自己的 transform

        foreach (var child in father)
        {
            Matrix4x4 tbone = Matrix4x4.TRS(child.transform.position, child.transform.rotation,
                child.transform.lossyScale);
            trans.Add(child.name, tbone);
        }

        return trans;
    }


    // **********************Compute Loss****************************
    private float CalcuDistanceRatio(Matrix4x4 a, Matrix4x4 b)
    {
        // ans = Dab / (S1y+S2y)
        return Vector3.Distance(a.GetPosition(), b.GetPosition()) / (a.GetScale().y + b.GetScale().y);
    }

    // **********************Fetch Loss Location Point****************************

    public static List<Dictionary<string, Vector3>> PosRecord = new List<Dictionary<string, Vector3>>();

    private Dictionary<string, Vector3> getAverage()
    {
        Dictionary<string, Vector3> tAver = new Dictionary<string, Vector3>();

        List<string> bonesName = new List<string> {"LeftMiddleDistal", "RightMiddleDistal", "LeftHand", "RightHand"};
        foreach (string bone in bonesName)
        {
            float x = 0, y = 0, z = 0;
            // 抛弃前1/5帧
            for (int i = PosRecord.Count / 5; i < PosRecord.Count; ++i)
            {
                var pos = PosRecord[i];

                x += pos[bone].x;
                y += pos[bone].y;
                z += pos[bone].z;

                if (bone == "LeftMiddleDistal" && isDrawPoint)
                {
                    if (float.IsNaN(pos[bone][0]))
                    {
                        Debug.Log("Get point postion failed.[getAverage]");
                    }
                    else
                    {
                        drawPoint(pos[bone], Color.blue);
                    }
                }
            }

            int count = PosRecord.Count - PosRecord.Count / 5;
            tAver.Add(bone, new Vector3(x, y, z) / count);
        }

        return tAver;
    }

    private float ComputeAll(Dictionary<string, Vector3> dic)
    {
        float sum = 0;
        foreach (string s in dic.Keys)
        {
            sum += dic[s].y;
        }

        return sum;
    }

    private Dictionary<string, Vector3> getLowest()
    {
        Dictionary<string, Vector3> tAver = new Dictionary<string, Vector3>();

        foreach (var pos in PosRecord)
        {
            if (ProcessText.PosRecord.Count == 1)
            {
                tAver = ProcessText.PosRecord[0];
            }
            else if (ComputeAll(pos) <= ComputeAll(tAver))
            {
                tAver = pos;
            }
        }

        return tAver;
    }

    // ************************Draw Compare Point**************************

    public bool isDrawPoint = false;

    public void drawPoint(Vector3 point, Color c)
    {
        if (!isDrawPoint) return;
        if (point != null)
        {
            GameObject tr = GameObject.CreatePrimitive(PrimitiveType.Sphere);
            tr.transform.position = point;
            tr.transform.localScale = new Vector3(0.06f, 0.06f, 0.06f);
            Material m_tr = tr.GetComponent<Renderer>().material;
            m_tr.color = c;

            foreach (var t in tr.GetComponents<Collider>())
            {
                Destroy(t);
            }


            GameObject pa = GameObject.Find("DispPoints");
            if (pa == null) pa = new GameObject("DispPoints");
            tr.transform.parent = pa.transform;
        }
    }

    private void clearPoint()
    {
        GameObject pa = GameObject.Find("DispPoints");
        if (pa != null)
        {
            int childCount = pa.transform.childCount;
            for (int i = 0; i < childCount; i++)
            {
                Destroy(pa.transform.GetChild(i).gameObject);
            }
        }
    }

    private void drawPlane(float x1, float y1, float x2, float y2, Color c)
    {
        Vector3 pos = new Vector3((x1 + x2) / 2, 0.1f, (y1 + y2) / 2);
        //UltiDraw.DrawWireQuad(pos,new Quaternion (), y1 - y2, x1 - x2, c);

        UltiDraw.DrawWireCuboid(pos, new Quaternion(), new Vector3(x1 - x2, y1 - y2, 0.1f), c);
    }


    private float calcuOutRange(int objUp, int objDown)
    {
        GameObject objU = info.models[objUp].model;
        GameObject objD = info.models[objDown].model;


        // 如果倾斜的矩形需要获取四个点来定位，改一下接口即可，暂不考虑
        Vector4 projUp = objU.GetComponent<VoxelCollider>().GetProjectionOnGround();
        Vector4 projDown = objD.GetComponent<VoxelCollider>().GetProjectionOnGround();

        float maxx = Mathf.Min(projUp.x, projDown.x);
        float maxy = Mathf.Min(projUp.y, projDown.y);
        float minx = Mathf.Max(projUp.z, projDown.z);
        float miny = Mathf.Max(projUp.w, projDown.w);

        float Soverlap = Mathf.Abs((maxx - minx) * (maxy - miny));
        float Starget = Mathf.Abs((projUp.x - projUp.z) * (projUp.y - projUp.w));

        drawPlane(projUp.x, projUp.y, projUp.z, projUp.w, Color.blue);
        drawPlane(projDown.x, projDown.y, projDown.z, projDown.w, Color.red);

        return Starget - Soverlap;
        //maxx = min{ maxx1,maxx2}
        //maxy = min{ maxy1,maxy2}
        //minx = max{ minx1,minx2}
        //miny = max{ miny1,miny2}
    }

    public float lambdaD = 1; // 碰撞距离
    public float lambdaR = 0.2f; // 临近关系
    public float lambdaC = 1; // 接触点距离

    public float disTarSty = 1f;
    public float disTar = 0f;

    Vector3 BonePointDirect = new Vector3();

    private float CalculateError(out Vector3 loss)
    {
        // 计算误差
        // 初始化参数


        //1.碰撞深度
        float costD = 0;
        foreach (var tmp in optimCur.collisionDepth.Values)
        {
            // Vector3 direct = optimCur.collisionDepth[key].Item1 - optimCur.collisionDepth[key].Item2;
            costD += 1.0f * tmp.Item1.Count / tmp.Item2;
        }

        //2.物体之间碰撞(有versupport关系)
        // 所有有versupport关系的物体，需要有投影面积被包含关系
        // loss2 = 1 - e^{-x}
        float loss2 = 0;
        //foreach (var tmp in optimCur.targetObj)
        //{
        //    if (tmp.Value != -1)
        //    {
        //        float St = calcuOutRange(tmp.Key, tmp.Value);
        //        loss2 += 1 - Mathf.Exp(-St);
        //    }
        //}

        //foreach (var tmp in optimCur.styleTargetObj)
        //{
        //    if (tmp.Value != -1)
        //    {
        //        float St = calcuOutRange(tmp.Key, tmp.Value);
        //        loss2 += 1 - Mathf.Exp(-St);
        //    }
        //}

        // loss = /Sigma | ln ( Ccur / Cori ) |
        //float costR = 0;
        //foreach (var tmp in optimCur.targetObj)
        //{
        //    if (bufferOld.targetObj.ContainsKey(tmp.Key))
        //    {
        //        if (bufferOld.targetObj[tmp.Key] == -1) continue;
        //        float c1 = CalcuDistanceRatio(optimCur.objTrans[tmp.Key], optimCur.objTrans[tmp.Value]);
        //        float c2 = CalcuDistanceRatio(bufferOld.objTrans[tmp.Key], bufferOld.objTrans[tmp.Value]);
        //        costR += Mathf.Abs(Mathf.Log(c1) - Mathf.Log(c2));
        //    }
        //}
        //foreach (var tmp in optimCur.styleTargetObj)
        //{
        //    if (bufferOld.styleTargetObj.ContainsKey(tmp.Key))
        //    {
        //        if (bufferOld.styleTargetObj[tmp.Key] == -1) continue;
        //        float c1 = CalcuDistanceRatio(optimCur.objTrans[tmp.Key], optimCur.objTrans[tmp.Value]);
        //        float c2 = CalcuDistanceRatio(bufferOld.objTrans[tmp.Key], bufferOld.objTrans[tmp.Value]);
        //        costR += Mathf.Abs(Mathf.Log(c1) - Mathf.Log(c2));
        //    }
        //}


        //3.contact离对应Bone的距离
        clearPoint();

        //Dictionary<string, Vector3> point_adopted = new Dictionary<string, Vector3>(getLowest());
        Dictionary<string, Vector3> point_adopted = new Dictionary<string, Vector3>(getAverage());

        // optimCur.lastOptimFrame;
        float costC = 0;
        // Dictionary<string, string> boneMaps = new Dictionary<string, string> { { "RightWrist", "mixamorig7:LeftHand" }, { "LeftWrist", "mixamorig7:RightHand" },
        // { "Hips", "mixamorig7:Hips" }, { "LMidHand", "mixamorig7:LeftHandMiddle4" }, { "RMidHand", "mixamorig7:RightHandMiddle4" } };
        Dictionary<string, string> boneMaps_hasStyleObj = new Dictionary<string, string>
            {{"LMidHand", "LeftMiddleDistal"}, {"RMidHand", "RightMiddleDistal"}};
        Dictionary<string, string> boneMaps_nonStyleObj = new Dictionary<string, string>
            {{"RightWrist", "LeftHand"}, {"LeftWrist", "RightHand"}};

        // clear sphere

        if (bufferAction.hasTarget)
        {
            Transform[] father = tarInterModel.GetComponentsInChildren<Transform>();
            foreach (var child in father)
            {
                if (child.name.Equals("Hips"))
                {
                    bool findHips = false;
                    foreach (var bonename in optimCur.lastOptimFrame.Keys)
                    {
                        if (bonename.Contains("Hips"))
                        {
                            Vector3 tbMod = optimCur.lastOptimFrame[bonename].GetPosition();

                            costC += disTar * Vector3.Distance(child.transform.position, tbMod);

                            if (float.IsNaN(tbMod[0]))
                            {
                                Debug.Log("Get point postion failed.[CalculateError/bufferAction.hasTarget]");
                            }
                            else
                            {
                                drawPoint(tbMod, Color.red);
                            }
                            findHips = true;
                            break;
                        }
                    }

                    if (!findHips) Debug.LogError("No Hips in the ");
                }

                //if (boneMaps.ContainsKey(child.name))
                //{
                //    Vector3 tbMod = optimCur.lastOptimFrame[boneMaps[child.name]].GetPosition();
                //    costC += Vector3.Distance(child.transform.position, tbMod);
                //}
            }
        }

        // 启发式check
        BonePointDirect = Vector3.zero;


        if (bufferAction.hasStyleObj)
        {
            Transform[] father = tarStyleModel.GetComponentsInChildren<Transform>();
            foreach (var child in father)
            {
                if (boneMaps_hasStyleObj.ContainsKey(child.name) &&
                    point_adopted.ContainsKey(boneMaps_hasStyleObj[child.name]))
                {
                    Vector3 tbMod = point_adopted[boneMaps_hasStyleObj[child.name]];
                    //Vector3 tbMod = optimCur.lastOptimFrame[boneMaps_hasStyleObj[child.name]].GetPosition();
                    costC += disTarSty * Vector3.Distance(child.transform.position, tbMod);

                    // 启发式check
                    //isBonePointLower = child.transform.position.y < tbMod.y - 0.2;
                    BonePointDirect = tbMod  - child.transform.position;



                    if (float.IsNaN(tbMod[0]))
                    {
                        Debug.Log("Get point postion failed.[CalculateError/bufferAction.hasStyleObj]");
                    }
                    else
                    {
                        drawPoint(tbMod, Color.red);
                    }
                }
            }
        }
        else
        {
            Transform[] father = tarInterModel.GetComponentsInChildren<Transform>();
            foreach (var child in father)
            {
                if (boneMaps_nonStyleObj.ContainsKey(child.name) &&
                    point_adopted.ContainsKey(boneMaps_nonStyleObj[child.name]))
                {
                    Vector3 tbMod = point_adopted[boneMaps_nonStyleObj[child.name]];
                    //Vector3 tbMod = optimCur.lastOptimFrame[boneMaps_hasStyleObj[child.name]].GetPosition();
                    costC += Vector3.Distance(child.transform.position, tbMod);

                    if (float.IsNaN(tbMod[0]))
                    {
                        Debug.Log("Get point postion failed.[CalculateError/ bufferAction.hasStyleObj/ false]");
                    }
                    else
                    {
                        drawPoint(tbMod, Color.red);
                    }
                }
            }
        }


        // ********************************************
        // 迭代完成重置
        PosRecord = new List<Dictionary<string, Vector3>>();
        // *********************************************

        float cost = lambdaD * costD + lambdaR * loss2 + lambdaC * costC;
        loss = new Vector3(lambdaD * costD, lambdaR * loss2, lambdaC * costC);
        //OptimLog($"loss1: {costD}  loss2: {costR}  loss3: {costC}");
        return cost;
    }

    // *****************Encapsulated interface function*********************************

    private void OptimLog(string str)
    {
        TextDialog.textDebug(str);
        using (StreamWriter sw = new StreamWriter("OptimizeLog.txt", true))
        {
            sw.WriteLine(str);
        }
    }

    private void HeuristicOptimLog(string str)
    {
        Debug.Log(str);
        using (StreamWriter sw = new StreamWriter("HeuristicOptimizeLog.txt", true))
        {
            sw.WriteLine(str);
        }
    }

    public bool IsOptim()
    {
        return isOptim;
    }

    public bool IsNSMOptim()
    {
        return isNSMOptim;
    }

    public bool IsResetOptim()
    {
        return isResetOptim;
    }

    public bool IsStayingActionBreak()
    {
        return stayingActionBreak;
    }


    public void setActResetCompleted(bool b)
    {
        actResetCompleted = b;
        Debug.Log("==========>detect 2");
    }

    public void setStayingActionBreak(bool b)
    {
        stayingActionBreak = b;
    }
}